/*
 * CostMarker.cpp
 *
 *  Created on: Oct 21, 2010
 *      Author: sanjeev
 */

#include "CostMarker.h"
#include "constants.h"

CostMarker::CostMarker() {
	point.header.frame_id = "/map";

	point.ns = "costs";
	point.action = visualization_msgs::Marker::ADD;
	point.pose.orientation.w = 1.0;

	point.id = 0;

	point.type = visualization_msgs::Marker::POINTS;

	// point markers use x and y scale for width/height respectively
	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.scale.z = 0.1f;

	// point are yellow
	point.color.g = 1.0f;
	point.color.r = 1.0f;
	point.color.a = 1.0f;
	point.lifetime = ros::Duration(0);
}

CostMarker::~CostMarker() {
	// TODO Auto-generated destructor stub
}

void CostMarker::Plot(MATRIX &inp, ros::Publisher &pub, double grain){

	point.points.clear();
	int height = inp.size();
	int width = inp[0].size();

	double min = INF, max = -INF, temp;

	for(int i=0;i<height;i++){
		for(int j=0;j<width;j++){
			min = inp[i][j] > 0.0 ? std::min(min,inp[i][j]) : min;
			max = inp[i][j] < INF/10 ? std::max(max,inp[i][j]) : max;
		}
	}

	//ROS_INFO("Plotting with max as %lf and min as %lf",max,min);

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;
	for(int i = 0; i<height; ++i) {
		for(int j=0;j<width;++j) {
			temp = inp[i][j];
			if(temp == 0 || temp == INF ) {
				continue;
			}else {
				c.r = (temp-min+1)/(max-min+1);
				c.b = (max-temp+1)/(max-min+1);
				//c.g = 0.0;
			}
			c.a = 1.0;
			p.x = j * grain;
			p.y = i * grain;
			p.z = 0;
			point.colors.push_back(c);
			point.points.push_back(p);
		}
	}
	pub.publish(point);
}

void CostMarker::PlotPoses(int i, int j, ros::Publisher &pub, double grain){
	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.ns = "path";

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;
	c.r = 0.0;
	c.b = 0.0;
	c.g = 1.0;
	c.a = 1.0;
	p.x = (double)j * grain;
	p.y = (double)i * grain;
	//ROS_INFO("Plot point %lf %lf",p.x,p.y);
	p.z = 0;
	point.colors.push_back(c);
	point.points.push_back(p);
	pub.publish(point);

	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.scale.z = 0.1f;

}


void CostMarker::PlotPath(std::vector<PII> path, ros::Publisher &pub, double grain)	{

	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.ns = "path";

	geometry_msgs::Point p;
	std_msgs::ColorRGBA c;
	c.r = 0.0;
	c.b = 0.0;
	c.g = 1.0;
	c.a = 1.0;
	//ROS_INFO("Plot point %lf %lf",p.x,p.y);
	p.z = 0;

	point.scale.x = 0.1f;
	point.scale.y = 0.1f;
	point.scale.z = 0.1f;
		
	for (int i=0; i<path.size(); ++i)	{
		p.x = path[i].second * grain;
		p.y = path[i].first * grain;
		
		ROS_INFO("Plot point %lf %lf",p.x,p.y);
			
		point.colors.push_back(c);
		point.points.push_back(p);
	}
	
	pub.publish(point);
	
}
